#include "State_Jump.h"
#include "Dog.h"

#define KP1 8.0f
#define KD1 1.3f
#define KP2 6.0f
#define KD2 1.3f

void Jump_Enter(void)
{
    UART_Print("Jump enter\n");
}

void Jump_Run(void)
{
    // UART_Print("Jump run\n");

    /* 曲腿准备 */
    HT_SetPosition(dog.legs[0].motorF, ANGLE_F + global.jumpAngle, KP1, KD1, 0);
    HT_SetPosition(dog.legs[1].motorF, -ANGLE_F - global.jumpAngle, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorF, -ANGLE_F - global.jumpAngle, KP2, KD2, 0);
    HT_SetPosition(dog.legs[3].motorF, ANGLE_F + global.jumpAngle, KP2, KD2, 0);

    HT_SetPosition(dog.legs[0].motorB, -ANGLE_B - global.jumpAngle, KP1, KD1, 0);
    HT_SetPosition(dog.legs[1].motorB, ANGLE_B + global.jumpAngle, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorB, ANGLE_B + global.jumpAngle, KP2, KD2, 0);
    HT_SetPosition(dog.legs[3].motorB, -ANGLE_B - global.jumpAngle, KP2, KD2, 0);
}

void Jump_Exit(void)
{
    UART_Print("Jump exit\n");
}